"""          ----------------------------------------------------------------------------


     OpenSteer -- Steering Behaviors for Autonomous Characters

     Copyright (c) 2002-2003, Sony Computer Entertainment America
     Original author: Craig Reynolds <craig_reynolds@playstation.sony.com>

     Permission is hereby granted, free of charge, to any person obtaining a
     copy of this software and associated documentation files (the "Software"),
     to deal in the Software without restriction, including without limitation
     the rights to use, copy, modify, merge, publish, distribute, sublicense,
     and/or sell copies of the Software, and to permit persons to whom the
     Software is furnished to do so, subject to the following conditions:

     The above copyright notice and this permission notice shall be included in
     all copies or substantial portions of the Software.

     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
     IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
     FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
     THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
     LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
     FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
     DEALINGS IN THE SOFTWARE.

    Porting by Serge Besnard (serge [at] petiteappli [dot] com)

     ----------------------------------------------------------------------------
    """
from math import sqrt,pow
from random import random
import logging

class Vector2D:
    """ a stripped out class for 2D vectors"""
    
    
    def __mul__(self,s):
        """Returns a Vector2D result of multiplication by scalar s"""
        
        return Vector2D(tuple([x*s for x in self.V]))
    def __div__(self,s):
        """Returns a Vector2D result of multiplication by scalar s"""
        if not s==0 :
            return Vector2D([x/s for x in self.V])
        #ERROR DIVISION BY ZERO
        return Vector2D((0,0))
    
    def __add__(self,A):
        return Vector2D(tuple([x1+x2 for (x1,x2) in zip(self.V,A)])) 
    
    def __neg__(self):
        """Returns the oposite Vector2D """
        
        return Vector2D(tuple([-x for x in self.V]))
    
    def __sub__(self,A):
        return Vector2D(tuple([x1-x2 for (x1,x2) in zip(self.V,A)])) 
    
            
    def __init__(self, v):
        self.V=tuple([v[i] for i in range(2)])


    
        
    def __cmp__(self,v):
        return cmp(self.V,v.asTuple())
    def __iter__(self):
        return self.V.__iter__()
    
    

    
    def __eq__(self,v):
        if (self.V==tuple(v)):
            return True
        return False
        
    def asTuple(self):
        return self.V

    def __getitem__(self,i):
        return self.V[i]
        
    def dotProduct(self, b):
        """simple dot product function"""
        #more elegant, but not tested
        return sum([x1*x2 for (x1,x2) in zip(self.V,b)])
        #return sum([self.V[i]*b[i] for i in range(2)])
    
    def length(self):
        return sqrt(sum([x*x for x in self.V]))
    
    def normalize(self):
        fLength =  self.length()
        if fLength > 1e-08 :
            fInvLength = 1.0 / fLength
            self.V=tuple([x*fInvLength for x in self.V])
            return fLength
        return 0
    
    
    def __str__(self):
        return "Vector2D"+str(self.V)   

class Utilities:
    
    def parallelComponent (self, source, unitBasis):
        projection = source.dotProduct(unitBasis)
        return unitBasis * projection
    
    def clip(self,  x,  min,  max):
        
            if (x < min) : return min
            if (x > max) : return max
            return x
    
    def perpendicularComponent (self, source, unitBasis):
        """return component of vector perpendicular to a unit basis vector
        (IMPORTANT NOTE: assumes "basis" has unit magnitude (length==1))"""
        return source - self.parallelComponent(source,unitBasis)
    
    def interpolate(self, alpha, x0, x1):
        return x0 + ((x1 - x0) * alpha)
    
    def limitMaxDeviationAngle (self, source,cosineOfConeAngle,basis):
        
        return Utilities().vecLimitDeviationAngleUtility (True, source, cosineOfConeAngle,basis)
    
    def vecLimitDeviationAngleUtility(self,insideOrOutside,  source,  cosineOfConeAngle,basis):
        sourceLength = source.length()
        if sourceLength == 0 : return source
        
        # measure the angular diviation of "source" from "basis"
        direction = source *(1/ sourceLength)
        
        cosineOfSourceAngle = direction.dotProduct (basis)
        # Simply return "source" if it already meets the angle criteria.
        # (note: we hope this top "if" gets compiled out since the flag
        # is a constant when the function is inlined into its caller)
        if insideOrOutside:
            if cosineOfSourceAngle >= cosineOfConeAngle : return source
        else :
            # source vector is already outside the cone, just return it
            if cosineOfSourceAngle <= cosineOfConeAngle : return source
        
        #find the portion of "source" that is perpendicular to "basis"
        perp = Utilities().perpendicularComponent(source,basis)
        
        
        #normalize that perpendicular
        unitPerp = perp
        unitPerp.normalize()

        """# construct a new vector whose length equals the source vector,
            # and lies on the intersection of a plane (formed the source and
            # basis vectors) and a cone (whose axis is "basis" and whose
            # angle corresponds to cosineOfConeAngle)"""
        perpDist = sqrt (1 - (cosineOfConeAngle * cosineOfConeAngle))
        c0 = basis * cosineOfConeAngle
        c1 = unitPerp * perpDist
        return (c0 + c1) * sourceLength 

    def blendIntoAccumulator(self, smoothRate, newValue, smoothedAccumulator):
        return self.interpolate(Utilities().clip(smoothRate, 0., 1.),\
                    smoothedAccumulator,newValue)


        
class LocalSpace:
    """          ----------------------------------------------------------------------------


     OpenSteer -- Steering Behaviors for Autonomous Characters

     Copyright (c) 2002-2003, Sony Computer Entertainment America
     Original author: Craig Reynolds <craig_reynolds@playstation.sony.com>

     Permission is hereby granted, free of charge, to any person obtaining a
     copy of this software and associated documentation files (the "Software"),
     to deal in the Software without restriction, including without limitation
     the rights to use, copy, modify, merge, publish, distribute, sublicense,
     and/or sell copies of the Software, and to permit persons to whom the
     Software is furnished to do so, subject to the following conditions:

     The above copyright notice and this permission notice shall be included in
     all copies or substantial portions of the Software.

     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
     IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
     FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
     THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
     LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
     FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
     DEALINGS IN THE SOFTWARE.


     ----------------------------------------------------------------------------

    """
    
    ZERO=Vector2D((0.,0.))
    UNIT_X=Vector2D((1.,0.))
    UNIT_Y=Vector2D((0.,1.))
    
    
    def __init__(self,right_handed):

        self._rightHanded=right_handed
        self.position=Vector2D((0.,0.))
        self.side=Vector2D((0.,1.))
        self.forward=Vector2D((1.,0.))
        self.resetLocalSpace()
        
        
        
    
    def setForward(self, f):
        
        self.regenerateOrthonormalBasis(tuple([f[i] for i in range(2)]))
        return self.forward
    
    def setForwardFrom3D(self,f):
        f=Utilities().perpendicularComponent(Vector2D((f.x,f.y,f.z)),Vector2D((0.0,1.0,0.0)))
        return self.setForward(f)
    
    def setPosition(self, p):
          
        self.position=Vector2D(p)
        return self.position
        
   
    
        
    def rightHanded(self):
        "use right-(or left-)handed coordinate space"
        return self._rightHanded
    
    def resetLocalSpace(self):
        """ ------------------------------------------------------------------------
         reset transform: set local space to its identity state, equivalent to a
         4x4 homogeneous transform like this:
        
             [ X 0 0 0 ]
             [ 0 1 0 0 ]
             [ 0 0 1 0 ]
             [ 0 0 0 1 ]
        
         where X is 1 for a left-handed system and -1 for a right-handed system."""
        self.forward=Vector2D((1.,0.))
        self.side=self.localRotateForwardToSide (self.forward)
        
        self.position=Vector2D((0., 0.))
    
       
    
    
    def localizeDirection(self, globalDirection):
        """transform a direction in global space to its equivalent in local space"""
        if not (globalDirection is Vector2D) :
            globalDirection=Vector2D(globalDirection)
        
        return Vector2D((globalDirection.dotProduct(self.side),\
                globalDirection.dotProduct(self.forward)))
     
    def globalizePosition(self, localPosition):
        """transform a point in local space to its equivalent in global space"""
        return self.position+globalizeDirection (localPosition)

    def globalizeDirection(self, localDirection):
        """transform a direction in local space to its equivalent in global space"""
        return self.side*localDirection[0]+ self.forward*localDirection[2]
                                
    def setUnitSideFromForward(self):
        """set "side" basis vector to normalized cross product of forward and up"""
        
        self.side=self.localRotateForwardToSide(self.forward)
        self.side.normalize ()
    
    def regenerateOrthonormalBasisUF(self, newUnitForward):
        """ regenerate the orthonormal basis vectors given a new forward
           (which is expected to have unit length)"""
        
        if not (newUnitForward is Vector2D) : newUnitForward=Vector2D(newUnitForward)
                
        self.forward = newUnitForward

        # derive new side basis vector from NEW forward and OLD up
        self.setUnitSideFromForward ()

        
    def regenerateOrthonormalBasis(self, newForward):
        """for when the new forward is NOT know to have unit length"""
        
        if not (newForward is Vector2D) :
            newForward=Vector2D(newForward)
        
        newForward.normalize()
        self.regenerateOrthonormalBasisUF (newForward)    

    
        
    def localRotateForwardToSide (self, v):
        """rotate, in the canonical direction, a vector pointing in the
        "forward" (+Z) direction to the "side" (+/-X) direction"""
        
        if self.rightHanded :
            return Vector2D((-v[1],v[0]))
        return Vector2D((v[1],v[0]))
    

class AbstractVehicle(LocalSpace) :
    
        
    def  __init__(self, right_handed):
        LocalSpace.__init__(self,right_handed)
        self.mass=0.0
        self.speed=0.0
        self.radius=0.0
        self.velocity=LocalSpace.ZERO
        self.maxForce=0.0
        self.maxSpeed=0.0
        
        
    
    def setMass(self,mass):
        self.mass=mass

    # size of bounding sphere, for obstacle avoidance, etc.
    
    def setRadius(self,radius):
        self.radius=radius

    
    
    def setSpeed(self,speed) : self.speed=speed

    
    def predictFuturePosition(self, predictionTime) : 
        """ groups of (pointers to) abstract vehicles, and iterators over them
        #typedef std::vector<AbstractVehicle*> group;
        #typedef group::const_iterator iterator;    

        # predict position of this vehicle at some time in the future
        # (assumes velocity remains constant)"""
        return LocalSpace.ZERO
    
    

    
    
    def setMaxForce(self,max):
        self.maxForce=max
    

    
    
    def setMaxSpeed(self, max):
        
        self.maxSpeed=max
    
class PathIntersection:
    
    def setIntersect(self,value):
        self.intersect=value
    def setDistance(self,value):
        self.distance=value
    def setObstacle(self,value):
        self.obstacle=value
    
class Random():
    def nextFloat(self):
        return random()
    
class SteerLibrary (AbstractVehicle) :
    """ """
    
    def  __init__(self,right_handed) :
        AbstractVehicle.__init__(self,right_handed)
        self.gaudyPursuitAnnotation = False
        self.randomGenerator=Random()
        self.resetSteering()
    
    def resetSteering (self):
        self.wanderSide = 0
        self.wanderUp = 0

        #default to non-gaudyPursuitAnnotation
        self.gaudyPursuitAnnotation = False
        

        
    def isAhead ( self, target, cosThreshold) :
    
        targetDirection = (target - self.position)
        targetDirection.normalise()
        return (self.forward.dotProduct(targetDirection) > cosThreshold)
    
    def isAside ( self, target, cosThreshold) :
        targetDirection = (target - self.position)
        targetDirection.normalise ()
        dp = self.forward.dotProduct(targetDirection)
        return (dp < cosThreshold) and (dp > -cosThreshold)
    def frandom01 (self):
        return (self.randomGenerator.nextFloat())
        
    def scalarRandomWalk (self, initial, walkspeed,  min,  max):
        
        next = initial + (((self.frandom01() * 2) - 1) * walkspeed)
        if (next < min) : return min
        if (next > max) : return max
        return next
    
    def isBehind (self, target, cosThreshold) :
    
        targetDirection = (target - self.position)
        targetDirection.normalise ()
        return self.forward.dotProduct(targetDirection) < cosThreshold
    
        
    def steerForWander (self, dt):
        """random walk WanderSide and WanderUp between -1 and +1"""
        speed = 12 * dt
        self.wanderSide = self.scalarRandomWalk (self.WanderSide, speed, -1, 1)
        
        # return a pure lateral steering vector: (+/-Side) + (+/-Up)
        return (self.side * self.wanderSide) 
    
    def steerForSeek(self, target):
        """"""
        #logging.debug("Steer for seek")
        target=Vector2D(target)
        #logging.debug("target : "+str(target))
        
        desiredVelocity = target - self.position
        return desiredVelocity - self.velocity
    
    def truncateLength(self, tVector, maxLength):

        tLength = tVector.length()
        returnVector = tVector
        if tLength > maxLength:
            returnVector.normalize() # = tVector - tVector*(tLength - maxLength)
            returnVector = returnVector*maxLength
        return returnVector    
    
class SimpleVehicule(SteerLibrary):
    
    serialNumberCounter=0
    
    
    
    def  __init__(self,right_handed):
        print "SteerLibrary initit..."
        SteerLibrary.__init__(self,right_handed)
        #set inital state
        print "SteerLibrary initiated"
        self.reset()
        self.maxSpeed=10.0
        self.mass=1.0
        self.maxForce=0.8
 
        
        #maintain unique serial numbers
        SimpleVehicule.serialNumberCounter += 1
        self.serialNumber=SimpleVehicule.serialNumberCounter

    def velocity(self):
        return self.forward * self.speed

    
    
    def reset(self):
        self.resetLocalSpace()
        
        self.smoothedAcceleration=Vector2D((0,0))
        self._lastPosition=Vector2D((0,0))
        self._lastForward=self.forward
        self._curvature= self._smoothedCurvature=0
        self.smoothedPosition=self.position
        
    def adjustRawSteeringForce(self,force):
        maxAdjustedSpeed = 0.2 * self.maxSpeed

        if ((self.speed > maxAdjustedSpeed) or (force == Vector2D((0,0)))):
            return force
        else :
            range = self.speed / maxAdjustedSpeed
            #print "range :" +str(range)
            u=Utilities()
            cosine = u.interpolate(pow(range, 2), 1.0, -1.0)
            #print "force before "+str(force)
            #print "forward "+str(self.forward)
            #print "cosine "+str(cosine)
            tmp= u.limitMaxDeviationAngle(force, cosine, self.forward)
            #print "new force : "+str(tmp)
            #print ""
            
            return tmp
        
    def interpolate(self, alpha,  x0,  x1):
        
            return x0 + ((x1 - x0) * alpha)
        
    def regenerateLocalSpace(self,newVelocity):
        """// ----------------------------------------------------------------------------
        // the default version: keep FORWARD parallel to velocity, change UP as
        // little as possible.
        //
        // parameter names commented out to prevent compiler warning from "-W"
        """
        # adjust orthonormal basis vectors to be aligned with new velocity
        if self.speed > 0:
            #print self.speed
            self.regenerateOrthonormalBasisUF (newVelocity / self.speed)
        

    
    def measurePathCurvature(self,elapsedTime):
        """// ----------------------------------------------------------------------------
        // measure path curvature (1/turning-radius), maintain smoothed version
        """

        if elapsedTime > 0:
            
            dP = self._lastPosition - self.position;
            dF = (self._lastForward - self.forward ) / dP.length()
            #SI - BIT OF A WEIRD FIX HERE . NOT SURE IF ITS CORRECT
            #Vector3 lateral = dF.perpendicularComponent (forward ());
            lateral = Utilities().perpendicularComponent( dF,self.forward)

            if lateral.dotProduct(self.side) < 0 :
                sign=1.0
            else :
                sign=-1.0
            self._curvature = lateral.length() * sign
            #OpenSteerUtility.blendIntoAccumulator(elapsedTime * 4.0f, _curvature,_smoothedCurvature);
            self._smoothedCurvature= \
                Utilities().blendIntoAccumulator(elapsedTime * 4.0, \
                self._curvature, self._smoothedCurvature)

            self._lastForward = self.forward 
            self._lastPosition = self.position

    
    def applySteeringForce(self, force, elapsedTime):
    

        adjustedForce = self.adjustRawSteeringForce (force)

        """ enforce limit on magnitude of steering force
        #Vector3 clippedForce = adjustedForce.truncateLength (maxForce ());
        #Vector3 clippedForce = adjustedForce.truncateLength(maxForce());"""

           
        clippedForce = self.truncateLength(adjustedForce, self.maxForce)
        #print "clippedforce : "+str(clippedForce)
        # compute acceleration and velocity
        newAcceleration = clippedForce * (1/ self.mass)
        newVelocity = self.velocity

        # damp out abrupt changes and oscillations in steering acceleration
        # (rate is proportional to time step, then clipped into useful range)
        if elapsedTime > 0:
            smoothRate =Utilities().clip(9 * elapsedTime, 0.15, 0.4)
            self.smoothedAcceleration=Utilities().blendIntoAccumulator(\
                                smoothRate,\
                                  newAcceleration, self.smoothedAcceleration)
        
        #print "smotthed acceleration : "+str(self.smoothedAcceleration)
        
        # Euler integrate (per frame) acceleration into velocity
        newVelocity = newVelocity+self.smoothedAcceleration * elapsedTime

        # enforce speed limit
        
        #newVelocity = newVelocity.truncateLength (maxSpeed ());
        newVelocity = self.truncateLength(newVelocity,self.maxSpeed)
        #print "new Velocity : "+ str(newVelocity)

        # update Speed
        self.setSpeed (newVelocity.length())
        #logging.debug(str(newVelocity))
        
        # Euler integrate (per frame) velocity into position
        self.position+= (newVelocity * elapsedTime)
        #logging.debug(str(self.position))
        
        # regenerate local space (by default: align vehicle's forward axis with
        # new velocity, but this behavior may be overridden by derived classes.)
        self.regenerateLocalSpace(newVelocity)
        
        # maintain path curvature information
        self.measurePathCurvature (elapsedTime)

        # running average of recent positions
        self.smoothedPosition=Utilities().blendIntoAccumulator(elapsedTime * 0.06,\
                              self.position , self.smoothedPosition)

    def steerToAvoidNeighbors (self, minTimeToCollision,  others):
		"""// ----------------------------------------------------------------------------
		// Unaligned collision avoidance behavior: avoid colliding with other nearby
		// vehicles moving in unconstrained directions.  Determine which (if any)
		// other other vehicle we would collide with first, then steers to avoid the
		// site of that potential collision.  Returns a steering force vector, which
		// is zero length if there is no impending collision."""

	
		# first priority is to prevent immediate interpenetration
		separation = self.steerToAvoidCloseNeighbors (0, others)
		if (separation != LocalSpace.ZERO) : return separation

		# otherwise, go on to consider potential future collisions
		steer = 0
		threat = None

		# Time (in seconds) until the most immediate collision threat found
		# so far.  Initial value is a threshold: don't look more than this
		# many frames into the future.
		minTime = minTimeToCollision

		
		# for each of the other vehicles, determine which (if any)
		# pose the most immediate threat of collision.
		#for (AVIterator i = others.begin(); i != others.end(); i++)
		for other in others :
		
			if not other is self :
			
				# avoid when future positions are this close (or less)
				collisionDangerThreshold = self.radius * 2

				# predicted time until nearest approach of "this" and "other"
				time = self.predictNearestApproachTime (other)

				# If the time is in the future, sooner than any other
				# threatened collision...
				if ((time >= 0) and (time < minTime)):
				
					# if the two will be close enough to collide,
					# make a note of it
					cna=self.computeNearestApproachPositions (other, time)
					if cna[0]< collisionDangerThreshold:
					
						minTime = time
						threat = other
						xxxThreatPositionAtNearestApproach = cna[1][1]
						xxxOurPositionAtNearestApproach = cna[1][0]
						

		# if a potential collision was found, compute steering to avoid
		if threat: 
		
			# parallel: +1, perpendicular: 0, anti-parallel: -1
			parallelness = self.forward.dotProduct(threat.forward)
			angle = 0.707

			if parallelness < -angle:
			
				# anti-parallel "head on" paths:
				# steer away from future threat position
				offset = xxxThreatPositionAtNearestApproach - self.position
				sideDot = offset.dotProduct(self.side)
				if (sideDot > 0):
					steer=-1.0
				else:
					steer=1.0
			
			else :
			
				if (parallelness > angle):
				
					# parallel paths: steer away from threat
					offset = threat.position - self.position
					sideDot = offset.dotProduct(self.side)
					if (sideDot > 0):
						steer=-1.0
					else:
						steer=1.0
				
				else:
				
					# perpendicular paths: steer behind threat
					# (only the slower of the two does this)
					if threat.speed <= self.speed:
					
						sideDot = self.side.dotProduct(threat.velocity)
						if (sideDot > 0):
							steer=-1.0
						else:
							steer=1.0

		return self.side * steer

    def predictNearestApproachTime (self,other):
		"""// Given two vehicles, based on their current positions and velocities,
        # determine the time until nearest approach
        #
        # XXX should this return zero if they are already in contact?
		"""
        
		# imagine we are at the origin with no velocity,
		# compute the relative velocity of the other vehicle
		myVelocity = self.velocity
		otherVelocity = other.velocity
		relVelocity = otherVelocity - myVelocity
		relSpeed = relVelocity.length()

		# for parallel paths, the vehicles will always be at the same distance,
		# so return 0 (aka "now") since "there is no time like the present"
		if (relSpeed == 0) : return 0

		# Now consider the path of the other vehicle in this relative
		# space, a line defined by the relative position and velocity.
		# The distance from the origin (our vehicle) to that line is
		# the nearest approach.

		# Take the unit tangent along the other vehicle's path
		relTangent = relVelocity / relSpeed

		# find distance from its path to origin (compute offset from
		# other to us, find length of projection onto path)
		relPosition = self.position - other.position
		projection = relTangent.dotProduct(relPosition)

		return projection / relSpeed;

    def computeNearestApproachPositions (self, other, time):
		"""// Given the time until nearest approach (predictNearestApproachTime)
		// determine position of each vehicle at that time, and the distance
		// between them"""

		myTravel =       self.forward  *       self.speed  * time
		otherTravel = other.forward  * other.speed  * time
		myFinal =      self.position +    myTravel
		otherFinal = other.position  + otherTravel

		
		return tuple(len (myFinal - otherFinal),tuple(myFinal,otherFinal))
	
    def steerToAvoidCloseNeighbors (self, minSeparationDistance,  others):
        """// ----------------------------------------------------------------------------
        // avoidance of "close neighbors" -- used only by steerToAvoidNeighbors
        //
        // XXX  Does a hard steer away from any other agent who comes withing a
        // XXX  critical distance.  Ideally this should be replaced with a call
        // XXX  to steerForSeparation."""
        for other in others:
			if other is not self :
			
				sumOfRadii = self.radius + other.radius
				minCenterToCenter = minSeparationDistance + sumOfRadii
				offset = other.position - self.position
				currentDistance = offset.length()

				if currentDistance < minCenterToCenter:
					return Utilities().perpendicularComponent(-offset,self.forward)
	  
        return LocalSpace.ZERO
	

if __name__ == '__main__':
    print ("Start test")
    ls=SimpleVehicule(True)
    ls2=SimpleVehicule(True)
    ls2.setPosition((1,0))
    ls2.setForward((-1,0))
    ls.setForward((1,0))
    print ls.position
    
    for i in range(50):
        ls.applySteeringForce(ls.steerForSeek((1,1)),0.1)
        f=ls2.steerForSeek((0,1))+ls2.steerToAvoidCloseNeighbors(0.5,[ls,])
        ls2.applySteeringForce(f,0.1)
        
        print ls2.position
        
        
    
    